﻿#include "ParticleSwarm.h"
#include "Util.h"
#include <iostream>
#include <vector>
#include <algorithm>
#include <functional>

using namespace std;

ParticleSwarm::ParticleSwarm()
{
	swarmSize = NUM_TOTAL_PARTICLES;
	swarmDimension = vPOS_DOF;

	double minArray[NUM_DOF]={
		-70,-15,-35,	
		15,0,0,-15,	
		0,0,0,0,
		-10,0,0,0,	
		0,0,0,0,	
		0,0,0,0};

	double maxArray[NUM_DOF]={
		75,120,20,	
		60,90,50,80,	
		45,90,100,60,
		10,90,100,60,	
		-30,90,100,60,	
		-45,90,100,60};
	// Hand joint range of motion
	//hmin.wirst_Rotation = Eigen::Vector3f(-70,-15,-35); // gap/ngua - xoay - trai/phai
	//hmax.wirst_Rotation = Eigen::Vector3f(75,120,20);
	//
	//hmin.thumb_Rotation=Eigen::Vector4f(15,0,0,-15); // Mo, xoay, dot 2 (duoi), dot 1 (tren cung)
	//hmax.thumb_Rotation=Eigen::Vector4f(60,90,50,80);

	//hmin.finger1_Rotation = Eigen::Vector4f(0,0,0,0); // // trai phai, dot 3, dot 2, dot 1
	//hmax.finger1_Rotation = Eigen::Vector4f(45,90,100,60);

	//hmin.finger2_Rotation = Eigen::Vector4f(-10,0,0,0); 
	//hmax.finger2_Rotation = Eigen::Vector4f(10,90,100,60);

	//hmin.finger3_Rotation = Eigen::Vector4f(0,0,0,0); 
	//hmax.finger3_Rotation = Eigen::Vector4f(-30,90,100,60);

	//hmin.little_Rotation = Eigen::Vector4f(0,0,0,0);  
	//hmax.little_Rotation = Eigen::Vector4f(-45,90,100,60);

	for (int i=0;i<NUM_DOF;i++)
	{
		hmin.dofData[i]=minArray[i];
		hmax.dofData[i]=maxArray[i];
	}

	// Convert from Hypothesis to Position
	MinRange = HypoToPosition(hmin);
	MaxRange = HypoToPosition(hmax);

	TendencyToOwnBest = 2.8;
	TendencyToGlobalBest = 1.3;
	double gama = TendencyToOwnBest + TendencyToGlobalBest; 
	Momentum = 2/(abs(2-gama-sqrt(pow(gama,2)-4*gama))); // eq.4 - paper HandTracking - Oikonomidis
	PercentMaximumVelocityOfSearchSpace = 1.0;
	UseGlobalOptimum = true;
}


ParticleSwarm::~ParticleSwarm()
{
	vector<Particle*>::iterator iter;
	for (iter = swParticles.begin(); iter != swParticles.end(); ++iter)
	{
		delete *iter;
	}

}

void ParticleSwarm::InitSwarm(HandMeasureFunction *function)
{
	this->swCostFunction = function;
	//swCostFunction->hypoArrays.resize(64);
	vector<Particle*> particles;
	for (int i=0; i < swarmSize; i++)
	{
		/// Initialize the position and velocity
		vector<double> position;
		vector<double> velocity;
		for (int j=0; j < swarmDimension; j++)
		{
			double x = MinRange[j] + (MaxRange[j] - MinRange[j])*_rnd.NextDouble();
			double vx = 0; //MinRange[j] + (MaxRange[j] - MinRange[j])*_rnd.NextDouble();
			position.push_back(x);
			velocity.push_back(vx);
		}
		particles.push_back(new Particle(this,position,velocity));
	}
	

	for (int i=0;i<swarmSize;i++)
	{
		swCostFunction->hypoArrays.push_back(PositionXToHypo(particles[i]->pPosition));
	}
	//GPU run
	vector<double> measureResult(swarmSize);

	swCostFunction->psoFunction(measureResult);
	this->swParticles = particles;
	for (int i=0;i<swarmSize;i++)	
	{
		swParticles[i]->update_Data(measureResult[i],TRUE);		
	}
	

	SortParticles();
	gBestCost = CurrentgBestCost();
	gBestPosition = CurrentgBestPosition();
}

// Execute one evolution step
void ParticleSwarm::Iteration()
{
	// Fitness is calculated for each particle, and update their personal best position
	for (int i = 0; i < swarmSize; i++)
	{
		// Update new position and velocity for each particle
		if (UseGlobalOptimum)
		{
			swParticles[i]->UpdateVelocityAndPosition(gBestPosition);
		}
		else
		{
			swParticles[i]->UpdateVelocityAndPosition(CurrentgBestPosition());
		}
		
	}
	for (int i=0;i<swarmSize;i++)	swCostFunction->hypoArrays[i] = PositionXToHypo(swParticles[i]->pPosition);
	
	vector<double> measureResult(swarmSize);
	swCostFunction->psoFunction(measureResult);
	
	for (int i=0;i<swarmSize;i++)	swParticles[i]->update_Data(measureResult[i],FALSE);
	
	// Update gBest
	SortParticles();
	if (CurrentgBestCost() < gBestCost)
	{
		gBestPosition = CurrentgBestPosition();
		gBestCost = CurrentgBestCost();
	}
}

// Randomize particles to avoid swarm collapse
void ParticleSwarm::Randomization(double randPercent, int lowBoundDim, int highBoundDim)
{
	double randProbability = 0.5;
	int	randSize = int(swarmSize*randPercent);
	int swarmOffset = swarmSize-randSize;
	swCostFunction->hypoArrays.resize(randSize);
	//update X particles (max-x) -> max

	for (int i = swarmOffset; i < swarmSize; i++){
		int j = lowBoundDim + (int)((highBoundDim - lowBoundDim)*_rnd.NextDouble());
		double x = MinRange[j] + (MaxRange[j] - MinRange[j])*_rnd.NextDouble();
		swParticles[i]->pPosition[j] = x;
		swParticles[i]->pVelocity[j] = 0; 
	
		//update hypo array 0 -> x
		swCostFunction->hypoArrays[i-swarmOffset] = PositionXToHypo(swParticles[i]->pPosition);
	}
	vector<double> measureResult(randSize);
	swCostFunction->psoFunction(measureResult);

	//update result 0->x to particles (max-x)->max
	for (int i=0;i<randSize;i++)	swParticles[swarmOffset+i]->update_Data(measureResult[i],FALSE);
	swCostFunction->hypoArrays.resize(swarmSize);// set size for normal rendering

	// Update gBest
	SortParticles();
	if (CurrentgBestCost() < gBestCost)
	{
		gBestPosition = CurrentgBestPosition();
		gBestCost = CurrentgBestCost();
	}
}

vector<double> ParticleSwarm::CurrentgBestPosition()
{
	return swParticles[0]->pPosition;
}


double ParticleSwarm::CurrentgBestCost()
{
	return swParticles[0]->pCost;
}


Particle& ParticleSwarm::operator [](int i)
{
	return *(swParticles.at(i));
}

void ParticleSwarm::SortParticles()
{
	std::sort(swParticles.begin(),swParticles.end(),CompareTo);
}